/*
文件名：pid.cpp
时间：2013-9-11
功能：
	通用pid和简单系统辨识
*/
#include "pid.h"//认为main.h中已经包含了math.h

#define PID		(*p)
//死区PID(带抗饱和积分,积分分离,不完全微分（适合噪声严重的系统）)
float base_pid(float e,PID_CON *p)//传入误差
{
	float tmpu=0;	 //基础输出
	if(fabs(e) < PID.die) e=0; //死区
	PID.e=e*PID.k_in + PID.e*(1-PID.k_in); //对误差低通滤波
	//PID
	PID.up=PID.e * PID.P;
	PID.ui=PID.e_acc;
	PID.ud=PID.ud * (1-PID.Dc) + ((PID.e - PID.e_pre) * PID.D / PID.T) * PID.Dc;//不完全微分
	//小范围加速逼近（P增加:1+2*sqrt(1-(e/K))）
	if(PID.high_e>0.000001) //P变参系数
	{
		float tmpu=PID.e/PID.high_e; //归一化
		if(PID.e>=0)
		{//y=1 - (x-1)^2
			if(PID.e<PID.high_e)
			{
				tmpu=tmpu-1;
				tmpu=1-tmpu*tmpu;
				PID.up=tmpu*PID.high_e*PID.P;
			}
		}
		else
		{//y=(x+1)^2 - 1
			if(PID.e>-PID.high_e)
			{
				tmpu=tmpu+1;
				tmpu=tmpu*tmpu-1;
				PID.up=tmpu*PID.high_e*PID.P;
			}
		}
	}
	//d限幅
	MINMAX(PID.ud,-PID.max_d,PID.max_d);
	//准备数据
	PID.e_pre=PID.e;
	tmpu = PID.uf + PID.up + PID.ui + PID.ud;//输出基础+P+I+D
	float di=e*PID.I*PID.T; //积分使用原始无滤波的e
	if(tmpu > PID.max)//限幅+抗饱和积分
	{
		tmpu=PID.max;
		if(tmpu*PID.e_acc>=0) di=0; //只允许I环节削弱(相反)，不允许增强了(同号)
	}
	else if(tmpu < PID.min)
	{
		tmpu=PID.min;
		if(tmpu*PID.e_acc>=0) di=0; //只允许I环节削弱(相反)，不允许增强了(同号)
	}
	if(fabs(e) > PID.E && e*PID.e_acc>=0) //积分分离：只允许I环节削弱(相反)，不允许增强了(同号)
	{
		di=0;
	}
	PID.e_acc+=di;
	//积分值限幅（抑制超调）
	MINMAX(PID.e_acc,-PID.max_i,PID.max_i);
	//输出
	PID.u=tmpu;
	return PID.u;
}
////////////////////////////////////////////////////////////////////////////////
//速度辨识
void pull_learn(float u,float v,PULL_REC *sr) //模型辨识
{
	if(sr->stat<10) //收集数据
	{
		sr->vacc+=v; sr->uacc+=u; sr->stat++;
	}
	else //执行
	{
		float cv=sr->vacc / sr->stat;
		float cu=sr->uacc / sr->stat; //当前的数据，是10次的平均
		float dv=cv - sr->v1; //速度差
		float a=dv/sr->Ts/sr->stat; //加速度
		float au=(cu + sr->u1)/2; //平均力矩
		sr->v1=cv;
		sr->u1=cu;
		a=a+sr->fk; //牵引力总加速度
		float F=au* sr-> u_F; //牵引力
		float m=0;
		if(fabs(a)>0.1f && fabs(F)>50) //若能计算
		{
			m=F/a;
			if(m>0) sr->m += sr->learn_speed * (m - sr->m);
		}
		sr->vacc=0; sr->uacc=0; sr->stat=0;
	}
}

